/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include "Map.h"

#include <mutex>

namespace ORB_SLAM2
{

std::string Map::sDatabase = "ORB_SLAM2";
std::string Map::sCollectionKF = "KeyFrame";
std::string Map::sCollectionMP = "MapPoint";
const std::string sUri = "mongodb://localhost:27017";

Map::Map(KeyFrameDatabase* pDB, const std::string uri, bool drop):
    mpMongoIO(new MongoIO::IO(uri)), mpKeyFrameDB(pDB), mnMaxKFid(0), mnBigChangeIdx(0), mbKFBuffClear(false), mbMPBuffClear(false)
{
    //! clear old data
    if (drop)
    {
        std::cout << "\n\nClearring Data in " << Map::sDatabase << std::endl;
        mpMongoIO->DropDatabase(Map::sDatabase);
    }
}

void Map::AddKeyFrame(KeyFrame *pKF)
{
    {
        unique_lock<mutex> lock(mMutexMap);
        mspKeyFrames.insert(pKF);
        if(pKF->mnId>mnMaxKFid)
            mnMaxKFid=pKF->mnId;
    }

    const bsoncxx::types::b_oid oid = pKF->WriteToDataBase();
    std::cout << "Write KeyFrame " << pKF->mnId << " to database, get oid: " << oid.value.to_string() << std::endl;

    SetDatabaseKeyFrame(pKF);
}

void Map::AddMapPoint(MapPoint *pMP)
{
    {
        unique_lock<mutex> lock(mMutexMap);
        mspMapPoints.insert(pMP);
    }

    const bsoncxx::types::b_oid oid = pMP->WriteToDataBase();
//    std::cout << "Write MapPoint " << pMP->mnId << " to database, get oid: " << oid.value.to_string() << std::endl;

    SetDatabaseMapPoint(pMP);
}

KeyFrame* Map::AddKeyFrame(const MongoIO::ID &id)
{
    //! In case the KeyFrame is alreadly exist
    KeyFrame* pKF = GetDatabaseKeyFrame(id.oid());
    if(pKF)
        return pKF;

    LoadKeyFrame lKF(id, this);
    pKF = new KeyFrame(lKF);
    //std::cout << "Load KeyFrame " << pKF->mnId << " form database, oid: " << id.oid().value.to_string() << std::endl;

    {
        unique_lock<mutex> lock(mMutexMap);
        mspKeyFrames.insert(pKF);
        if (pKF->mnId > mnMaxKFid)
            mnMaxKFid = pKF->mnId;
    }

    return pKF;
}

MapPoint* Map::AddMapPoint(bsoncxx::document::view &view, const MongoIO::ID &id)
{
    //! In case the MapPoint is alreadly exist
    MapPoint* pMP = GetDatabaseMapPoint(id.oid());
    if(pMP)
        return pMP;

    pMP = new MapPoint(view, id, this);
    //std::cout << "Load MapPoint " << pMP->mnId << " form database, oid: " << id.oid().value.to_string() << std::endl;

    {
        unique_lock<mutex> lock(mMutexMap);
        mspMapPoints.insert(pMP);
    }

    SetDatabaseMapPoint(pMP);

    return pMP;
}

void Map::EraseMapPoint(MapPoint *pMP)
{
    unique_lock<mutex> lock(mMutexMap);
    mspMapPoints.erase(pMP);

    {
        unique_lock<mutex> loc1(mMutexDatabase);
        mmDataBaseMapPoints.erase(pMP->MongoID().oid().value.to_string());
    }
    pMP->EraseFromDataBase();
    std::cout << "Erase MapPoint " << pMP->mnId << " from database" << std::endl;
    // TODO: This only erase the pointer.
    // Delete the MapPoint
}

void Map::EraseKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexMap);
    mspKeyFrames.erase(pKF);

    mmDataBaseKeyFrames.erase(pKF->MongoID().oid().value.to_string());
    pKF->EraseFromDataBase();
    std::cout << "Erase KeyFrame " << pKF->mnId << " from database" << std::endl;
    // TODO: This only erase the pointer.
    // Delete the MapPoint
}

void Map::SetReferenceMapPoints(const vector<MapPoint *> &vpMPs)
{
    unique_lock<mutex> lock(mMutexMap);
    mvpReferenceMapPoints = vpMPs;
}

void Map::InformNewBigChange()
{
    unique_lock<mutex> lock(mMutexMap);
    mnBigChangeIdx++;
}

int Map::GetLastBigChangeIdx()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnBigChangeIdx;
}

vector<KeyFrame*> Map::GetAllKeyFrames()
{
    unique_lock<mutex> lock(mMutexMap);
    return vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
}

vector<MapPoint*> Map::GetAllMapPoints()
{
    unique_lock<mutex> lock(mMutexMap);
    return vector<MapPoint*>(mspMapPoints.begin(),mspMapPoints.end());
}

long unsigned int Map::MapPointsInMap()
{
    unique_lock<mutex> lock(mMutexMap);
    return mspMapPoints.size();
}

long unsigned int Map::KeyFramesInMap()
{
    unique_lock<mutex> lock(mMutexMap);
    return mspKeyFrames.size();
}

vector<MapPoint*> Map::GetReferenceMapPoints()
{
    unique_lock<mutex> lock(mMutexMap);
    return mvpReferenceMapPoints;
}

long unsigned int Map::GetMaxKFid()
{
    unique_lock<mutex> lock(mMutexMap);
    return mnMaxKFid;
}

void Map::clear()
{
    for(set<MapPoint*>::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++)
        delete *sit;

    for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(), send=mspKeyFrames.end(); sit!=send; sit++)
        delete *sit;

    mspMapPoints.clear();
    mspKeyFrames.clear();
    mnMaxKFid = 0;
    mvpReferenceMapPoints.clear();
    mvpKeyFrameOrigins.clear();
}

void Map::SetDatabaseKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> loc1(mMutexDatabase);
    mmDataBaseKeyFrames.insert(std::pair<std::string, KeyFrame*>(pKF->MongoID().oid().value.to_string(), pKF));
}

void Map::SetDatabaseMapPoint(MapPoint *pMP)
{
    unique_lock<mutex> loc1(mMutexDatabase);
    mmDataBaseMapPoints.insert(std::pair<std::string, MapPoint *>(pMP->MongoID().oid().value.to_string(), pMP));
}

KeyFrame* Map::GetDatabaseKeyFrame(const bsoncxx::types::b_oid &oid)
{
    unique_lock<mutex> loc1(mMutexDatabase);
    const std::string soid = oid.value.to_string();
    if(mmDataBaseKeyFrames.count(soid) > 0)
        return mmDataBaseKeyFrames[soid];
    else
        return nullptr;
}

MapPoint* Map::GetDatabaseMapPoint(const bsoncxx::types::b_oid &oid)
{
    unique_lock<mutex> loc1(mMutexDatabase);
    const std::string soid = oid.value.to_string();
    if(mmDataBaseMapPoints.count(soid) > 0)
        return mmDataBaseMapPoints[soid];
    else
        return nullptr;
}

void Map::AddKeyFrameUpdateBuffer(KeyFrame *pKF, const std::string para)
{
    unique_lock<mutex> loc1(mMutexKeyFrameBuffer);

    if(mbKFBuffClear)
    {
        mmKeyFrameSaveBuffer.clear();
        mbKFBuffClear = false;
    }

    mmKeyFrameSaveBuffer[pKF].insert(para);
}

void Map::AddMapPointUpdateBuffer(MapPoint *pMP, const std::string para)
{
    unique_lock<mutex> loc1(mMutexMapPointBuffer);

    if(mbMPBuffClear)
    {
        mmMapPointSaveBuffer.clear();
        mbMPBuffClear = false;
    }

    mmMapPointSaveBuffer[pMP].insert(para);
}

std::map<KeyFrame*, std::set<std::string> > Map::GetKeyFrameUpdateBuffer()
{
    unique_lock<mutex> loc1(mMutexKeyFrameBuffer);

    if(!mmKeyFrameSaveBuffer.empty() && mbKFBuffClear)
    {
        mmKeyFrameSaveBuffer.clear();
    }

    mbKFBuffClear = true;
    return mmKeyFrameSaveBuffer;
}

std::map<MapPoint*, std::set<std::string> > Map::GetMapPointUpdateBuffer()
{
    unique_lock<mutex> loc1(mMutexMapPointBuffer);

    if(!mmMapPointSaveBuffer.empty() && mbMPBuffClear)
    {
        mmMapPointSaveBuffer.clear();
    }

    mbMPBuffClear = true;
    return mmMapPointSaveBuffer;
}

} //namespace ORB_SLAM
